import sys
import types
import time
from collections import deque
from typing import List, Tuple
import numpy as np


# 在导入 test2 之前，注入一个假的 ultralytics 模块，避免真实依赖
dummy_ultralytics = types.ModuleType('ultralytics')


class _DummyYOLO:
    def __init__(self, *args, **kwargs) -> None:
        pass

    def predict(self, *args, **kwargs):
        # 返回空结果占位，不会被使用到
        return []


dummy_ultralytics.YOLO = _DummyYOLO
sys.modules['ultralytics'] = dummy_ultralytics

# 同目录导入 DetectModel（不会真正使用 YOLO）
import test2  # noqa: E402


class FakeCamera:
    """
    假相机：
    - 返回固定尺寸的 RGB 与深度图
    - 深度图全局填充当前距离值，便于 ROI 取平均
    - 提供固定相机内参
    """

    def __init__(self, width: int = 640, height: int = 480, start_distance_m: float = 2.0) -> None:
        self.width = width
        self.height = height
        self.current_distance_m = float(start_distance_m)
        self._rgb = np.zeros((height, width, 3), dtype=np.uint8)

    def get_align_rgb(self):
        return self._rgb

    def get_align_depth(self):
        depth = np.full((self.height, self.width), self.current_distance_m, dtype=np.float32)
        return depth

    def get_color_intrinsics(self):
        return (600.0, 600.0, 320.0, 240.0)


class FakeDetectModel(test2.DetectModel):
    """
    基于 DetectModel 的假实现：
    - detect() 直接返回模拟的目标类别与概率
    - measureDistance() 使用假相机深度图与固定 bbox 计算距离
    - send_velocity_command()/turn() 仅打印，便于观察流程
    """

    def __init__(
        self,
        target_label: str = 'ball',
        prob_threshold: float = 0.8,
        cache_size: int = 10,
        timeout: float = 2.0,
        start_distance_m: float = 2.0,
    ) -> None:
        # 不调用父类构造，避免真实加载 YOLO；手动初始化所需字段
        self.model = None
        self.prob_threshold = prob_threshold
        self.cache_size = cache_size
        self.timeout = timeout
        self.result_cache = deque(maxlen=cache_size)
        self.last_update_time = time.time()

        self.target_label = target_label
        self.camera = FakeCamera(start_distance_m=start_distance_m)

        # 固定 bbox（图像中心区域）
        w, h = self.camera.width, self.camera.height
        box_w, box_h = int(w * 0.2), int(h * 0.2)
        x1 = w // 2 - box_w // 2
        y1 = h // 2 - box_h // 2
        x2 = x1 + box_w
        y2 = y1 + box_h
        self.fake_bbox = (x1, y1, x2, y2)

        # 跟踪循环中的“逼近速度”与“偶发丢失”配置
        self.distance_step_m = 0.2  # 每次测距后接近 0.2m
        self.miss_every_n_steps = 30  # 每 N 次测距模拟一次丢失
        self._measure_count = 0

    # 统一相机接口
    def get_camera_frame(self):
        return self.camera.get_align_rgb()

    def get_align_depth(self):
        return self.camera.get_align_depth()

    # 模拟检测：恒定返回目标名称与足够高的概率
    def detect(self, frame) -> List[Tuple[str, float]]:
        return [(self.target_label, max(self.prob_threshold, 0.9))]

    # 模拟测距：
    # - 使用 ROI 在假深度图中做平均（其实全图等值即可）
    # - 每次调用后让目标“更近一些”；按周期模拟一次丢失
    def measureDistance(self, target_object: str):
        self._measure_count += 1
        # 模拟偶发丢失
        if self.miss_every_n_steps > 0 and self._measure_count % self.miss_every_n_steps == 0:
            return None

        # 读取深度并计算 ROI 平均值
        depth_map = self.get_align_depth()
        x1, y1, x2, y2 = self.fake_bbox
        roi_depth = depth_map[y1:y2, x1:x2]
        if roi_depth.size == 0:
            return None
        avg_distance = float(np.mean(roi_depth))

        # 让目标“靠近”
        self.camera.current_distance_m = max(0.0, self.camera.current_distance_m - self.distance_step_m)

        return target_object, 0.95, avg_distance, self.fake_bbox

    # 控制接口（仅打印）
    def send_velocity_command(self, vel_x: float, vel_y: float, yaw_z: float):
        print(f'[FakeDetectModel] speed: vx={vel_x:.2f}, vy={vel_y:.2f}, yaw_z={yaw_z:.2f}')

    def turn(self, angle: float):
        print(f'[FakeDetectModel] turn: angle={angle}')


if __name__ == '__main__':
    # 运行步骤：持续跟踪到 0.1m 或模拟丢失触发重新定位
    model = FakeDetectModel(target_label='ball', start_distance_m=2.0)
    model.GlobalManager('ball')


